
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_navigation.h
  * @author     baiyang
  * @date       2023-7-13
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <assert.h>

#include <common/location/location.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
// add new navigation controllers to this enum. Users can then
// select which navigation controller to use by setting the
// NAV_CONTROLLER parameter
enum NavControllerType {
    NAV_CONTROLLER_DEFAULT      = 0,
    NAV_CONTROLLER_L1           = 1
};

typedef struct navigation_controller *navctrl_t;
typedef const struct navigation_controller *navctrl_const_t;
typedef struct navigation_controller_ops *navctrl_ops_t;

/** @ 
  * @brief  
  */
struct navigation_controller_ops {
    // return the desired roll angle in centi-degrees to move towards
    // the target waypoint
    int32_t (*nav_roll_cd)(navctrl_const_t nav_ctrl);

    // return the desired lateral acceleration in m/s/s to move towards
    // the target waypoint
    float (*lateral_acceleration)(navctrl_const_t nav_ctrl);

    // note: all centi-degree values returned in AP_Navigation should
    // be wrapped at -18000 to 18000 in centi-degrees.

    // return the tracking bearing that the navigation controller is
    // using in centi-degrees. This is used to display an arrow on
    // ground stations showing the effect of the cross-tracking in the
    // controller
    int32_t (*nav_bearing_cd)(navctrl_const_t nav_ctrl);

    // return the difference between the vehicles current course and
    // the nav_bearing_cd() in centi-degrees. A positive value means
    // the vehicle is tracking too far to the left of the correct
    // bearing.
    int32_t (*bearing_error_cd)(navctrl_const_t nav_ctrl);

    // return the target bearing in centi-degrees. This is the bearing
    // from the vehicles current position to the target waypoint. This
    // should be calculated in the update_*() functions below.
    int32_t (*target_bearing_cd)(navctrl_const_t nav_ctrl);

    // return the crosstrack error in meters. This is the distance in
    // the X-Y plane that we are off the desired track
    float (*crosstrack_error)(navctrl_const_t nav_ctrl);
    float (*crosstrack_error_integrator)(navctrl_const_t nav_ctrl);

    // return the distance in meters at which a turn should commence
    // to allow the vehicle to neatly move to the next track in the
    // mission when approaching a waypoint. Assumes 90 degree turn
    float (*turn_distance)(navctrl_const_t nav_ctrl, float wp_radius);

    // return the distance in meters at which a turn should commence
    // to allow the vehicle to neatly move to the next track in the
    // mission when approaching a waypoint
    float (*turn_distance2)(navctrl_const_t nav_ctrl, float wp_radius, float turn_angle);

    // return the target loiter radius for the current location that
    // will not cause excessive airframe loading
    float (*loiter_radius)(navctrl_const_t nav_ctrl, const float radius);

    // update the internal state of the navigation controller, given
    // the previous and next waypoints. This is the step function for
    // navigation control for path following between two points.  This
    // function is called at regular intervals (typically 10Hz). The
    // main flight code will call an output function (such as
    // nav_roll_cd()) after this function to ask for the new required
    // navigation attitude/steering.
    // float dist_min = 0.0f
    void (*update_waypoint)(navctrl_t nav_ctrl, const Location *prev_WP, const Location *next_WP, float dist_min);

    // update the internal state of the navigation controller for when
    // the vehicle has been commanded to circle about a point.  This
    // is the step function for navigation control for circling.  This
    // function is called at regular intervals (typically 10Hz). The
    // main flight code will call an output function (such as
    // nav_roll_cd()) after this function to ask for the new required
    // navigation attitude/steering.
    void (*update_loiter)(navctrl_t nav_ctrl, const Location *center_WP, float radius, int8_t loiter_direction);

    // update the internal state of the navigation controller, given a
    // fixed heading. This is the step function for navigation control
    // for a fixed heading.  This function is called at regular
    // intervals (typically 10Hz). The main flight code will call an
    // output function (such as nav_roll_cd()) after this function to
    // ask for the new required navigation attitude/steering.
    void (*update_heading_hold)(navctrl_t nav_ctrl, int32_t navigation_heading_cd);

    // update the internal state of the navigation controller for
    // level flight on the current heading. This is the step function
    // for navigation control for level flight.  This function is
    // called at regular intervals (typically 10Hz). The main flight
    // code will call an output function (such as nav_roll_cd()) after
    // this function to ask for the new required navigation
    // attitude/steering.
    void (*update_level_flight)(navctrl_t nav_ctrl);

    // return true if we have reached the target loiter location. This
    // may be a fuzzy decision, depending on internal navigation
    // parameters. For example the controller may return true only
    // when on the circular path around the waypoint, and not when
    // tracking towards the center. This function is only valid when
    // the update_loiter() method is used
    bool (*reached_loiter_target)(navctrl_t nav_ctrl);

    // notify Navigation controller that a new waypoint has just been
    // processed. This means that until we handle an update_XXX() function
    // the data is stale with old navigation information.
    void (*set_data_is_stale)(navctrl_t nav_ctrl);

    // return true if a new waypoint has been processed by mission
    // controller but the navigation controller still has old stale data
    // from previous waypoint navigation handling. This gets cleared on
    // every update_XXXXXX() call.
    bool (*data_is_stale)(navctrl_const_t nav_ctrl);

    void (*set_reverse)(navctrl_t nav_ctrl, bool reverse);
};

/** @ 
  * @brief  
  */
struct navigation_controller {
    navctrl_ops_t ops;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// return the desired roll angle in centi-degrees to move towards
// the target waypoint
static inline int32_t navctrl_nav_roll_cd(navctrl_const_t nav_ctrl) {
    assert(nav_ctrl->ops->nav_roll_cd != NULL);
    return nav_ctrl->ops->nav_roll_cd(nav_ctrl);
}

// return the desired lateral acceleration in m/s/s to move towards
// the target waypoint
static inline float navctrl_lateral_acceleration(navctrl_const_t nav_ctrl) {
    assert(nav_ctrl->ops->lateral_acceleration != NULL);
    return nav_ctrl->ops->lateral_acceleration(nav_ctrl);
}

// note: all centi-degree values returned in AP_Navigation should
// be wrapped at -18000 to 18000 in centi-degrees.

// return the tracking bearing that the navigation controller is
// using in centi-degrees. This is used to display an arrow on
// ground stations showing the effect of the cross-tracking in the
// controller
static inline int32_t navctrl_nav_bearing_cd(navctrl_const_t nav_ctrl) {
    assert(nav_ctrl->ops->nav_bearing_cd != NULL);
    return nav_ctrl->ops->nav_bearing_cd(nav_ctrl);
}

// return the difference between the vehicles current course and
// the nav_bearing_cd() in centi-degrees. A positive value means
// the vehicle is tracking too far to the left of the correct
// bearing.
static inline int32_t navctrl_bearing_error_cd(navctrl_const_t nav_ctrl) {
    assert(nav_ctrl->ops->bearing_error_cd != NULL);
    return nav_ctrl->ops->bearing_error_cd(nav_ctrl);
}

// return the target bearing in centi-degrees. This is the bearing
// from the vehicles current position to the target waypoint. This
// should be calculated in the update_*() functions below.
static inline int32_t navctrl_target_bearing_cd(navctrl_const_t nav_ctrl) {
    assert(nav_ctrl->ops->target_bearing_cd != NULL);
    return nav_ctrl->ops->target_bearing_cd(nav_ctrl);
}

// return the crosstrack error in meters. This is the distance in
// the X-Y plane that we are off the desired track
static inline float navctrl_crosstrack_error(navctrl_const_t nav_ctrl) {
    assert(nav_ctrl->ops->crosstrack_error != NULL);
    return nav_ctrl->ops->crosstrack_error(nav_ctrl);
}
static inline float navctrl_crosstrack_error_integrator(navctrl_const_t nav_ctrl) {
    assert(nav_ctrl->ops->crosstrack_error_integrator != NULL);
    return nav_ctrl->ops->crosstrack_error_integrator(nav_ctrl);
}

// return the distance in meters at which a turn should commence
// to allow the vehicle to neatly move to the next track in the
// mission when approaching a waypoint. Assumes 90 degree turn
static inline float navctrl_turn_distance(navctrl_const_t nav_ctrl, float wp_radius) {
    assert(nav_ctrl->ops->turn_distance != NULL);
    return nav_ctrl->ops->turn_distance(nav_ctrl, wp_radius);
}

// return the distance in meters at which a turn should commence
// to allow the vehicle to neatly move to the next track in the
// mission when approaching a waypoint
static inline float navctrl_turn_distance2(navctrl_const_t nav_ctrl, float wp_radius, float turn_angle) {
    assert(nav_ctrl->ops->turn_distance2 != NULL);
    return nav_ctrl->ops->turn_distance2(nav_ctrl, wp_radius, turn_angle);
}

// return the target loiter radius for the current location that
// will not cause excessive airframe loading
static inline float navctrl_loiter_radius(navctrl_const_t nav_ctrl, const float radius) {
    assert(nav_ctrl->ops->loiter_radius != NULL);
    return nav_ctrl->ops->loiter_radius(nav_ctrl, radius);
}

// update the internal state of the navigation controller, given
// the previous and next waypoints. This is the step function for
// navigation control for path following between two points.  This
// function is called at regular intervals (typically 10Hz). The
// main flight code will call an output function (such as
// nav_roll_cd()) after this function to ask for the new required
// navigation attitude/steering.
// float dist_min = 0.0f
static inline void navctrl_update_waypoint(navctrl_t nav_ctrl, const Location *prev_WP, const Location *next_WP, float dist_min) {
    assert(nav_ctrl->ops->update_waypoint != NULL);
    nav_ctrl->ops->update_waypoint(nav_ctrl, prev_WP, next_WP, dist_min);
}

// update the internal state of the navigation controller for when
// the vehicle has been commanded to circle about a point.  This
// is the step function for navigation control for circling.  This
// function is called at regular intervals (typically 10Hz). The
// main flight code will call an output function (such as
// nav_roll_cd()) after this function to ask for the new required
// navigation attitude/steering.
static inline void navctrl_update_loiter(navctrl_t nav_ctrl, const Location *center_WP, float radius, int8_t loiter_direction) {
    assert(nav_ctrl->ops->update_loiter != NULL);
    nav_ctrl->ops->update_loiter(nav_ctrl, center_WP, radius, loiter_direction);
}

// update the internal state of the navigation controller, given a
// fixed heading. This is the step function for navigation control
// for a fixed heading.  This function is called at regular
// intervals (typically 10Hz). The main flight code will call an
// output function (such as nav_roll_cd()) after this function to
// ask for the new required navigation attitude/steering.
static inline void navctrl_update_heading_hold(navctrl_t nav_ctrl, int32_t navigation_heading_cd) {
    assert(nav_ctrl->ops->update_heading_hold != NULL);
    nav_ctrl->ops->update_heading_hold(nav_ctrl, navigation_heading_cd);
}

// update the internal state of the navigation controller for
// level flight on the current heading. This is the step function
// for navigation control for level flight.  This function is
// called at regular intervals (typically 10Hz). The main flight
// code will call an output function (such as nav_roll_cd()) after
// this function to ask for the new required navigation
// attitude/steering.
static inline void navctrl_update_level_flight(navctrl_t nav_ctrl) {
    assert(nav_ctrl->ops->update_level_flight != NULL);
    nav_ctrl->ops->update_level_flight(nav_ctrl);
}

// return true if we have reached the target loiter location. This
// may be a fuzzy decision, depending on internal navigation
// parameters. For example the controller may return true only
// when on the circular path around the waypoint, and not when
// tracking towards the center. This function is only valid when
// the update_loiter() method is used
static inline bool navctrl_reached_loiter_target(navctrl_t nav_ctrl) {
    assert(nav_ctrl->ops->reached_loiter_target != NULL);
    return nav_ctrl->ops->reached_loiter_target(nav_ctrl);
}

// notify Navigation controller that a new waypoint has just been
// processed. This means that until we handle an update_XXX() function
// the data is stale with old navigation information.
static inline void navctrl_set_data_is_stale(navctrl_t nav_ctrl) {
    assert(nav_ctrl->ops->set_data_is_stale != NULL);
    nav_ctrl->ops->set_data_is_stale(nav_ctrl);
}

// return true if a new waypoint has been processed by mission
// controller but the navigation controller still has old stale data
// from previous waypoint navigation handling. This gets cleared on
// every update_XXXXXX() call.
static inline bool navctrl_data_is_stale(navctrl_const_t nav_ctrl) {
    assert(nav_ctrl->ops->data_is_stale != NULL);
    return nav_ctrl->ops->data_is_stale(nav_ctrl);
}

static inline void navctrl_set_reverse(navctrl_t nav_ctrl, bool reverse) {
    assert(nav_ctrl->ops->set_reverse != NULL);
    nav_ctrl->ops->set_reverse(nav_ctrl, reverse);
}
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



